/**
 * @file servo.c
 * @author SunWater
 * @brief 舵机控制
 * @version 0.1
 * @date 2024-10-21
 *
 * @copyright Copyright (c) 2024
 *
 */

#include <servo.h>

#include "string.h"

#define PI 3.14159265358979323846f

void Servo_init(Servo *servo_p, Servo_Config *servo_config) {
    // 初始化舵机的PWM
    PWM_init(&servo_p->servo_pwm, servo_config->servo_htim,
             servo_config->tim_channel);

    // 设置舵机的计数百分比大小
    servo_p->duty_percent_min = servo_config->duty_percent_min;
    servo_p->duty_percent_max = servo_config->duty_percent_max;

    // 设置舵机的角度限制
    servo_p->servo_degree_min = servo_config->servo_degree_min;
    servo_p->servo_degree_max = servo_config->servo_degree_max;
}

void Servo_spin_to_deg(Servo *servo_p, float degrees) {
    // 角度范围
    uint16_t degree_range =
        servo_p->servo_degree_max - servo_p->servo_degree_min;

    // 占空比范围
    float duty_range = servo_p->duty_percent_max - servo_p->duty_percent_min;

    // 计算占空比
    uint32_t duty =
        (degrees / degree_range * duty_range + servo_p->duty_percent_min) /
        100.0f * servo_p->servo_pwm.tim_period;

    // 设置PWM的占空比
    PWM_set_duty(&servo_p->servo_pwm, duty);
}

void Servo_spin_to_rad(Servo *servo_p, float rad) {
    float degrees = rad * 180.0f / PI;  // 将弧度转换为角度
    Servo_spin_to_deg(servo_p, degrees);
}
